#include "imu.h"

IMU_Type imu;
SMP_Type smp;
static uint8_t imu_rdbuf[16];

static void mean_filter(void);

void imu_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;
    i2c_config_t conf =
    {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = IMU_SDA,
        .sda_pullup_en = GPIO_PULLUP_DISABLE,
        .scl_io_num = IMU_SCLK,
        .scl_pullup_en = GPIO_PULLUP_DISABLE,
        .master.clk_speed = 100000,
        // .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */
    };

    i2c_param_config(i2c_master_port, &conf);
    i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);

    imu_read(WHO_AM_I, imu_rdbuf);//0x0f, 51
    imu.id = imu_rdbuf[0];

    /*Initialization Setting*/
    imu_write(CTRL_REG2, 0x03);//0x21
    imu_write(CTRL_REG3, 0xc0);//0x22
    imu_write(CTRL_REG4, 0x80);//0x23
    imu_write(CTRL_REG6, 0xa8);//0x25

    /*INT Setting*/
    imu_write(INT1_CFG, 0x2a);//0x30
    imu_write(INT1_THS, 0x36);//0x32
    imu_write(INT1_DURATION, 0x0);//0x33

    imu_write(INT2_CFG, 0x2a);//0x34
    imu_write(INT2_THS, 0x36);//0x36
    imu_write(INT2_DURATION, 0x0);//0x37

    /*Sensor Enable*/
    imu_write(CTRL_REG1, 0x57);//0x20
}

int16_t imu_write(uint8_t reg_addr, uint8_t data)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (LIS2DH12_IIC_Addr << 1) | WRITE_BIT, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, data, ACK_CHECK_DIS);
    i2c_master_stop(cmd);
    int16_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

int16_t imu_read(uint8_t reg_addr, uint8_t *data_rd)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (LIS2DH12_IIC_Addr << 1) | WRITE_BIT, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (LIS2DH12_IIC_Addr << 1) | READ_BIT, ACK_CHECK_EN);
    i2c_master_read_byte(cmd, data_rd, NACK_VAL);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

void accel_read(void)
{
    imu_read(OUT_X_L, imu_rdbuf + 1);
    imu_read(OUT_X_H, imu_rdbuf + 2);
    imu_read(OUT_Y_L, imu_rdbuf + 3);
    imu_read(OUT_Y_H, imu_rdbuf + 4);
    imu_read(OUT_Z_L, imu_rdbuf + 5);
    imu_read(OUT_Z_H, imu_rdbuf + 6);

    imu.ax = imu_rdbuf[1] | (imu_rdbuf[2] << 8);
    imu.ay = imu_rdbuf[3] | (imu_rdbuf[4] << 8);
    imu.az = imu_rdbuf[5] | (imu_rdbuf[6] << 8);

    smp.ax = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(imu_rdbuf + 1)); 
	smp.ay = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(imu_rdbuf + 3));
	smp.az = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(imu_rdbuf + 5));
}

static void mean_filter(void)
{
    int16_t x_sum = 0, y_sum = 0, z_sum = 0;
    for (uint8_t i = 0; i < FILTER_CNT; i ++)
    {
        accel_read();
        x_sum += smp.ax;
        y_sum += smp.ay;
        z_sum += smp.az;
    }
    smp.ax = x_sum / FILTER_CNT;
    smp.ay = y_sum / FILTER_CNT;
    smp.az = z_sum / FILTER_CNT;
}

void angle_calc(void)
{
    mean_filter();
    smp.ax = atan((int16_t)smp.ax / (int16_t)sqrt(pow(smp.ay, 2) + pow(smp.az, 2))) * DEGREE_CAL;
    smp.ay = atan((int16_t)smp.ay / (int16_t)sqrt(pow(smp.ax, 2) + pow(smp.az, 2))) * DEGREE_CAL;
    smp.az = atan((int16_t)smp.az / (int16_t)sqrt(pow(smp.ax, 2) + pow(smp.ay, 2))) * DEGREE_CAL;
    if (smp.az < 0)
	{
		smp.ax = 180 - smp.ax;
		smp.ay = 180 - smp.ay;
	}
}



